﻿using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;

namespace LaserScan
{
    public class SectionSolver
    {
        Matrix Mat = new Matrix();
        private static double pi = Math.PI;
        public double[] SectionCenter { get; set; }
        private static double[] ScanCenter = { 0, 0 };

        /// <summary>
        /// 椭圆拟合
        /// </summary>
        /// <param name="XY"></param>
        /// <param name="LoopTimes"></param>
        /// <param name="n"></param>
        /// <returns></returns>
        private double[] EllipseFit(List<double[]> XY, int LoopTimes, int n)
        {
            try
            {
                int nSize = XY.Count;
                if (nSize < 4)
                {
                    throw new Exception("断面点云不足，不能拟合椭圆！");
                }
                List<double> X = new List<double>();
                List<double> Y = new List<double>();
                for (int i = 0; i < nSize; i++)
                {
                    X.Add(XY[i][0]);
                    Y.Add(XY[i][1]);
                }
                //初始化权阵及未知数近似初始值
                double[][] P = Mat.CreateMatrix(1, nSize, 1);
                double[][] Ellipse = new double[4][];
                Ellipse[0] = new double[1] { (X.Min() + X.Max()) / 2 };//Xc
                Ellipse[1] = new double[1] { (Y.Min() + Y.Max()) / 2 };//Yc
                Ellipse[2] = new double[1] { 2.75 };//a
                Ellipse[3] = new double[1] { 2.75 };//b
                                                    //误差方程系数阵
                double[][] B = new double[nSize][];
                double[][] BTP = new double[4][];
                //误差方程常数项
                double[][] L = new double[nSize][];
                double Ibs = 0;
                for (int k = 0; k < LoopTimes; k++)
                {
                    for (int i = 0; i < nSize; i++)
                    {
                        B[i] = new double[4];
                        B[i][0] = 2 * (Ellipse[0][0] - X[i]) / Math.Pow(Ellipse[2][0], 2);
                        B[i][1] = 2 * (Ellipse[1][0] - Y[i]) / Math.Pow(Ellipse[3][0], 2);
                        B[i][2] = -2 * Math.Pow((Ellipse[0][0] - X[i]), 2) / Math.Pow(Ellipse[2][0], 3);
                        B[i][3] = -2 * Math.Pow((Ellipse[1][0] - Y[i]), 2) / Math.Pow(Ellipse[3][0], 3);
                        L[i] = new double[1] { 1 - Math.Pow((Ellipse[0][0] - X[i]) / Ellipse[2][0], 2) - Math.Pow((Ellipse[1][0] - Y[i]) / Ellipse[3][0], 2) };
                    }
                    double[][] BT = Mat.TransposeMatrix(B);
                    for (int i = 0; i < 4; i++)
                    {
                        BTP[i] = new double[nSize];
                        for (int j = 0; j < nSize; j++)
                        {
                            BTP[i][j] = BT[i][j] * P[j][0];
                        }
                    }
                    double[][] Nbb = Mat.Multiply(BTP, B);//法方程系数阵Nbb=B'PB,4*4,
                    double[][] W = Mat.Multiply(BTP, L);//法方程常数项W=B'PL,4*1
                    double[][] EllipseTemp = Mat.Multiply(Mat.InverseMatrix(Nbb), W);//未知数改正数X=inv(Nbb)*W,4*1
                    Ellipse = Mat.Plus(Ellipse, EllipseTemp);
                    //残差,n*1
                    double[][] V = new double[nSize][];
                    double sum = 0; double Psum = 0;
                    for (int i = 0; i < nSize; i++)
                    {
                        V[i] = new double[1] { Math.Abs(Math.Pow((Ellipse[0][0] - X[i]) / Ellipse[2][0], 2) + Math.Pow((Ellipse[1][0] - Y[i]) / Ellipse[3][0], 2) - 1) };
                        sum += Math.Pow(V[i][0], 2) * P[i][0];
                        Psum += P[i][0];
                    }
                    Ibs = Math.Sqrt(sum / Psum);//有效点中误差
                    if (n == 0)
                    {
                        P = Mat.CreateMatrix(1, nSize, 1);
                    }
                    else
                    {
                        for (int i = 0; i < nSize; i++)
                        {
                            if (V[i][0] <= n * Ibs)
                            {
                                P[i][0] = 1;
                            }
                            else
                            {
                                P[i][0] = 0;
                            }
                        }
                    }
                }
                double[] EllipseFitTemp = new double[4] { Ellipse[0][0], Ellipse[1][0], Ellipse[2][0], Ellipse[3][0] };
                return EllipseFitTemp;
            }
            catch (Exception)
            {

                return new double[4]{ 0,0,0,0};
            }
            
        }

        /// <summary>
        /// 圆拟合
        /// </summary>
        /// <param name="XYZ">去噪后点云</param>
        /// <param name="Center">椭圆中心</param>
        /// <param name="Direct">坐标顺序</param>
        /// <param name="Ibs">输出拟合后点云中误差</param>
        /// <returns>返回圆的中心和半径参数</returns>
        private double[] CircleFit(List<double[]> XY, double[] OriginCircle, int LoopTimes, int n)
        {
            try
            {
                int nSize = XY.Count;
                double[][] Circle = new double[3][];
                if (nSize < 4)
                {
                    return new double[] { };
                }
                List<double> X = new List<double>();
                List<double> Y = new List<double>();
                for (int i = 0; i < nSize; i++)
                {
                    X.Add(XY[i][0]);
                    Y.Add(XY[i][1]);
                }
                double[][] P = Mat.CreateMatrix(1, nSize, 1);
                Circle[0] = new double[1] { OriginCircle[0] }; Circle[1] = new double[1] { OriginCircle[1] }; Circle[2] = new double[1] { OriginCircle[2] };
                double[][] B = new double[nSize][];
                double[][] BTP = new double[3][];
                double[][] L = new double[nSize][];
                double[][] R = new double[nSize][];
                double Ibs = 0;
                for (int i = 0; i < LoopTimes; i++)
                {
                    for (int j = 0; j < nSize; j++)
                    {
                        R[j] = new double[1] { Math.Sqrt(Math.Pow(Circle[0][0] - X[j], 2) + Math.Pow(Circle[1][0] - Y[j], 2)) };
                        B[j] = new double[3] { (Circle[0][0] - X[j]) / R[j][0], (Circle[1][0] - Y[j]) / R[j][0], -1 };
                        L[j] = new double[1] { Circle[2][0] - R[j][0] };
                    }
                    double[][] BT = Mat.TransposeMatrix(B);
                    for (int k = 0; k < 3; k++)
                    {
                        BTP[k] = new double[nSize];
                        for (int j = 0; j < nSize; j++)
                        {
                            BTP[k][j] = BT[k][j] * P[j][0];
                        }
                    }
                    double[][] Nbb = Mat.Multiply(BTP, B);//法方程系数阵Nbb=B'PB,3*3
                    double[][] W = Mat.Multiply(BTP, L);//法方程常数项W=B'PL,n*1
                    double[][] InvNbb = Mat.InverseMatrix(Nbb);
                    if (InvNbb.Length == 0)
                    {
                        return new double[] { };
                    }
                    double[][] CircleTemp = Mat.Multiply(InvNbb, W);//未知数改正数X=inv(Nbb)*W,3*1
                    Circle = Mat.Plus(Circle, CircleTemp);
                    //残差,n*1
                    double[][] V = Mat.Minus(Mat.Multiply(B, CircleTemp), L);
                    double sum = 0; double Psum = 0;
                    for (int m = 0; m < nSize; m++)
                    {
                        sum += Math.Pow(V[m][0], 2) * P[m][0];
                        Psum += P[m][0];
                    }
                    Ibs = Math.Sqrt(sum / Psum);//有效点中误差
                    P = Mat.CreateMatrix(0, nSize, 1);
                    //if (i >= 0 & i < 4)
                    //{
                    for (int k = 0; k < nSize; k++)
                    {
                        if (Math.Abs(V[k][0]) <= Ibs)
                        {
                            P[k][0] = 1;
                        }
                        if (Math.Abs(V[k][0]) > Ibs & Math.Abs(V[k][0]) <= n * Ibs)
                        {
                            P[k][0] = Math.Exp(-0.5 * (Math.Pow(V[k][0], 2) / Math.Pow(Ibs, 2)));
                        }
                        if (Math.Abs(V[k][0]) > n * Ibs)
                        {
                            P[k][0] = 0;
                        }
                    }
                    //}
                    //else
                    //{
                    //    for (int k = 0; k < nSize; k++)
                    //    {
                    //        if (Math.Abs(V[k][0]) > 3 * Ibs)
                    //        {
                    //            P[k][0] = Math.Exp(-0.5 * (Math.Pow(V[k][0], 2) / Math.Pow(Ibs, 2)));
                    //        }
                    //        else
                    //        {
                    //            P[k][0] = 1;
                    //        }
                    //    }
                    //}
              
                }
                Circle = Mat.TransposeMatrix(Circle);
                double[] CircleResult = new double[3] { Circle[0][0], Circle[0][1], Circle[0][2] };
                return CircleResult;
            }
            catch (Exception)
            {

               return new double[3] { 0,0,2.75};
            }
            
        }
               
        /// <summary>
        /// 分段圆弧拟合
        /// </summary>
        /// <param name="EllipseFitDenoise">输入椭圆拟合去噪后的断面</param>
        /// <param name="DesignAngle">以断面中心为原点的设计角度，默认为[138,73,8,-8,-73,-138]</param>
        /// <param name="OriginalSegmentedArc">断面中心和设计半径，如[Xc,Yc,2.75]</param>
        /// <returns>返回5段圆弧的中心和半径参数</returns>
        public List<double[]> SegmentedArcFit(List<double[]> EllipseFitDenoise, double[] DesignAngle, double[] OriginalSegmentedArc)
        {
            List<double[]> SegmentedArcFitResult =new List<double[]>();
            for (int i = 0; i < DesignAngle.Length - 1; i++)
            {
                List<double[]> TempXZ = new List<double[]>();
                if (DesignAngle[1] > 180)
                {
                    for (int j = 0; j < EllipseFitDenoise.Count; j++)
                    {
                        if (EllipseFitDenoise[j][2] + 180 < DesignAngle[i] && EllipseFitDenoise[j][2] + 180 >= DesignAngle[i + 1])
                        {
                            TempXZ.Add(EllipseFitDenoise[j]);
                        }
                    }
                }
                else
                {
                    for (int j = 0; j < EllipseFitDenoise.Count; j++)
                    {
                        double angle= Math.Atan2((EllipseFitDenoise[j][0] - SectionCenter[0]), (EllipseFitDenoise[j][1] - SectionCenter[1])) * 180 / pi;//去噪后的第5列点与断面中心的方向，仍然以天顶方向为0度，左为负[-150,0]，右为正[0,-150]
                        if (angle < DesignAngle[i] && angle >= DesignAngle[i + 1])
                        {
                            TempXZ.Add(EllipseFitDenoise[j]);
                        }
                    }
                }
                double[] Circle = CircleFit(TempXZ, OriginalSegmentedArc, 6, 3);
                SegmentedArcFitResult.Add(Circle);
            }
            return SegmentedArcFitResult;
        }

        /// <summary>
        /// 求圆弧到断面中心的距离
        /// </summary>
        /// <param name="Circle">圆弧参数</param>
        /// <param name="Angle">左右方向</param>
        /// <param name="SectionCenter">断面中心</param>
        /// <returns>圆弧到断面中心距离</returns>
        private double GetDistanceNew(double[] Circle, double Angle, double[] SectionCenter)
        {
            if (Circle.Length == 0)
                return 0;
            double X0 = Circle[0] - SectionCenter[0];
            double Y0 = Circle[1] - SectionCenter[1];
            double r = Circle[2];
            double B = Math.Pow(X0, 2) + Math.Pow(Y0, 2) - Math.Pow(r, 2);
            double A = X0 * Math.Sin(Angle) + Y0 * Math.Cos(Angle);
            if (Math.Pow(A, 2) - B < 0)
                return 0;
            else
                return A + Math.Sqrt(Math.Pow(A, 2) - B);
        }

        /// <summary>
        /// 根据圆弧参数和断面中心求水平直径
        /// </summary>
        /// <param name="SegmentArc">五段圆弧参数</param>
        /// <param name="SectionCenter">断面中心</param>
        /// <returns>断面水平直径</returns>
        public double GetDiameterNew(List<double[]> SegmentArc, double[] SectionCenter)
        {
            return GetDistanceNew(SegmentArc[0], pi / 2, SectionCenter) + GetDistanceNew(SegmentArc[4], -pi / 2, SectionCenter);
        }

        /// <summary>
        /// 整体椭圆拟合去噪
        /// </summary>
        /// <param name="RawXZ">粗去噪后的点云</param>
        /// <returns>椭圆拟合去噪后的点云</returns>
        public List<double[]> EllipseFitDenoise(List<double[]> RawXZ)
        {
            if (RawXZ!=null)
            {
                double[] Ellipse = EllipseFit(RawXZ, 6, 3);
                SectionCenter = new double[] { Ellipse[0], Ellipse[1] };
                List<double[]> EllipseFitDenoiseResult = new List<double[]>();
                for (int i = 0; i < RawXZ.Count; i++)
                {
                    double V = Math.Abs(Math.Pow((Ellipse[0] - RawXZ[i][0]) / Ellipse[2], 2) + Math.Pow((Ellipse[1] - RawXZ[i][1]) / Ellipse[3], 2) - 1);
                    if (V < 0.02)
                    {
                        double[] XZTemp = RawXZ[i];
                        //XZTemp[4] = 
                        EllipseFitDenoiseResult.Add(XZTemp);
                    }
                }
                return EllipseFitDenoiseResult;
            }
            else
            {
                return null;
            }
           
        }


        /// <summary>
        /// 根据最大设计角度和距离进行断面粗去噪（去除轨道和飘散点）
        /// </summary>
        /// <param name="RawXZ">原始断面</param>
        /// <param name="DesignAngle">设计角度，此处默认为[138,73,8,-8,-73,-,138]</param>
        /// <param name="DistanceRange">设计距离，此处默认为[5,1]</param>
        /// <returns>粗去噪后的断面</returns>
        public List<double[]> CrudeDenoise(List<double[]> RawXZ, double[] DesignAngle, double[] DistanceRange)
        {
            if (RawXZ.Count == 0)
            {
                return null;
            }
            else
            {
                double sectionMaxDistance = DistanceRange[0]; double sectionMinDistance = DistanceRange[1];
                List<double[]> CrudeDenoiseXZResult =new List<double[]>();
                for (int i = 0; i < RawXZ.Count; i++)
                {
                    double angle=Math.Atan2(RawXZ[i][0], RawXZ[i][1]) * 180 / pi;
                    double distance= Math.Sqrt(Math.Pow(RawXZ[i][0] - ScanCenter[0], 2) + Math.Pow(RawXZ[i][1] - ScanCenter[1], 2));
                    if (distance<= sectionMaxDistance &&
                    distance >= sectionMinDistance &&
                    angle <= DesignAngle[0] &&
                    angle >= DesignAngle[DesignAngle.Length - 1])
                    {
                        CrudeDenoiseXZResult.Add(RawXZ[i]);
                    }
                }
                return CrudeDenoiseXZResult;
            }
        }

        public void OutSection(string FilePath, List<double[]> OutXY, int RingNum)
        {
            if (File.Exists(FilePath))
                return;
            else
            {
                FileStream fs = new FileStream(FilePath, FileMode.Create);
                StreamWriter writer = new StreamWriter(fs, System.Text.Encoding.Default);
                for (int i = 0; i < OutXY.Count; i++)
                {
                    string str = OutXY[i][0].ToString("0.0000") + "," + RingNum.ToString("0.0000") + "," + OutXY[i][1].ToString("0.0000");
                    writer.WriteLine(str);
                }
                writer.Flush();
                fs.Close();
            }
        }
        public double[] GetCuoTai(double[] SectionCenter, List<double[]> SegmentArc, double[] SeamAngle)
        {
            double[] DisLocation = new double[4];
            for (int i = 1; i < 5; i++)
            {
                if (SegmentArc[i - 1].Length != 0 && SegmentArc[i].Length != 0)
                {
                    DisLocation[i - 1] = CuoTaiCal(SegmentArc[i], SegmentArc[i - 1], SectionCenter, SeamAngle[i]);
                }
                else
                {
                    DisLocation[i - 1] = 999;
                }
            }
            return DisLocation;
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="Circle1"></param>
        /// <param name="Circle2"></param>
        /// <param name="SectionCenter"></param>
        /// <param name="SeamAngle"></param>
        /// <returns></returns>
        private double CuoTaiCal(double[] Circle1, double[] Circle2, double[] SectionCenter, double SeamAngle)
        {
            double distance = 0;
            double D1 = 0;
            double D2 = 0;
            double Angle1 = 0;
            double Angle2 = 0;
            double distance1 = 0;
            double distance2 = 0;
            D1 = Math.Sqrt(Math.Pow(Circle1[0] - SectionCenter[0], 2) + Math.Pow(Circle1[1] - SectionCenter[1], 2));
            D2 = Math.Sqrt(Math.Pow(Circle2[0] - SectionCenter[0], 2) + Math.Pow(Circle2[1] - SectionCenter[1], 2));
            Angle1 = SeamAngle * Math.PI / 180 - Math.Atan2(Circle1[0] - SectionCenter[0], Circle1[1] - SectionCenter[1]);
            Angle2 = SeamAngle * Math.PI / 180 - Math.Atan2(Circle2[0] - SectionCenter[0], Circle2[1] - SectionCenter[1]);
            distance1 = D1 * Math.Cos(Angle1) + Math.Sqrt(Math.Pow(Circle1[2], 2) - Math.Pow(D1, 2) * Math.Pow(Math.Sin(Angle1), 2));
            distance2 = D2 * Math.Cos(Angle2) + Math.Sqrt(Math.Pow(Circle2[2], 2) - Math.Pow(D2, 2) * Math.Pow(Math.Sin(Angle2), 2));
            distance = Math.Abs(distance1 - distance2) * 1000;
            return distance;
        }
    }
}
